#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S3, HTServo,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorFL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorFR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorBL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorBR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     motorFan1,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     motorFan2,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_1,     motorSweep,    tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servoFL,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_2,    servoBL,              tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_3,    servoTubeSlide,       tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_4,    servo1_3_4,           tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C3_5,    servo1_3_5,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo1_3_6,           tServoStandard)
#pragma config(Servo,  srvo_S2_C3_1,    servoFR,              tServoStandard)
#pragma config(Servo,  srvo_S2_C3_2,    servoBR,              tServoStandard)
#pragma config(Servo,  srvo_S2_C3_3,    servoLoader,           tServoStandard)
#pragma config(Servo,  srvo_S2_C3_4,    servo2_3_4,           tServoStandard)
#pragma config(Servo,  srvo_S2_C3_5,    servo2_3_5,           tServoStandard)
#pragma config(Servo,  srvo_S2_C3_6,    servo2_3_6,           tServoStandard)
#pragma config(Servo,  srvo_S3_C1_1,    servoSweepArm,          tServoContinuousRotation)
#pragma config(Servo,  srvo_S3_C1_2,    servoTubeWinch,          tServoContinuousRotation)
#pragma config(Servo,  srvo_S3_C1_3,    servoGrabber,          tServoContinuousRotation)
#pragma config(Servo,  srvo_S3_C1_4,    servo3_1_4,           tServoContinuousRotation)
#pragma config(Servo,  srvo_S3_C1_5,    servo3_1_5,           tServoStandard)
#pragma config(Servo,  srvo_S3_C1_6,    servo3_1_6,           tServoStandard)
#pragma config(Servo,  srvo_S3_C2_1,    servoSweep1,        tServoStandard)
#pragma config(Servo,  srvo_S3_C2_2,    servoSweep2,       tServoStandard)
#pragma config(Servo,  srvo_S3_C2_3,    servoTubeLift,         tServoStandard)
#pragma config(Servo,  srvo_S3_C2_4,    servo3_2_4,           tServoStandard)
#pragma config(Servo,  srvo_S3_C2_5,    servo3_2_5,           tServoStandard)
#pragma config(Servo,  srvo_S3_C2_6,    servo3_2_6,           tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "../includes/read_write.c"
#include "../JoystickDriver.c"
#include "../../library/drive_modes/simple_swerve_4m.c"
#include "../includes/manipulators.c"
//#include "../includes/fir.c"


task main()
{
	RegisterMotors(
	motorFL,
	motorFR,
	motorBL,
	motorBR,
	motorSweep,
	motorFan1,
	motorFan2
	);
	RegisterServos(
	servoFL,
	servoBL,
	servoFR,
	servoBR,
	servoGrabber,
	servoSweepArm,
	servoTubeWinch,
	servoTubeLift,
	servoTubeSlide,
	servoLoader,
	servoSweep1,
	servoSweep2,
	);

	tSensors touch = (tSensors) S4;
	initialize();

	float Kp = 0.009;
	float Ki = 0.008;
	float Kd = 0.005;
	float errorPrevSum[4] = {0.0, 0.0, 0.0, 0.0};
	float errorPrev[4] = {0.0, 0.0, 0.0, 0.0};
	float error[4];
	float ang[4] = {0.0, 0.0, 0.0, 0.0};
	float newAng[4] = {0.0, 0.0, 0.0, 0.0};
	float n[4] = {0.0, 0.0, 0.0, 0.0};
	float angPrev[4] = {0.0, 0.0, 0.0, 0.0};
	float newAngPrev[4] = {0.0, 0.0, 0.0, 0.0};
	float servoSpeed[4] = {0.0, 0.0, 0.0, 0.0};

	waitForStart();
	float joyDistance;
	float joyAngle;
	int joyX;
	int joyY;
	int joyZ;
	int MOTOR_SPEED = 90;
	servo[servoSweepArm] = 127;
	servo[servoTubeWinch] = 133;
	while(true)
	{
		bool fanRunning   = false;
		bool fanReady     = true;
		if (joystick.joy1_Buttons == 2)
		{
			//RunSweeper(true);
			motor[motorSweep] = -127;
			servo[servoSweep1] = 255;
			servo[servoSweep2] = 255;
		}
		else
		{
			//RunSweeper(false);
			servo[servoSweep1] = 127;
			servo[servoSweep2] = 127;
			motor[motorSweep] = 0;
		}


		//   !!!   begin goal grabber   !!!   //
		if (joystick.joy1_Buttons == 5)
		{
			servo[servoGrabber]= 50;
		}
		else if (joystick.joy1_Buttons == 4)
		{
			servo[servoGrabber] = 8;
		}
		///   !!!   end goal grabber   !!!   ///



		///   !!!   begin sweeper   !!!   ///

		///   !!!   end sweeper   !!!   ///


		///   !!!   begin tube   !!!   ///
		if (joystick.joy2_TopHat == 6)
		{
			servo[servoLoader] = 25;
			wait10Msec(25);
			servo[servoLoader] = 19;
		}
		else if (joystick.joy2_TopHat == 2)
		{
			servo[servoLoader] = 254;
			wait10Msec(25);
			servo[servoLoader] = 249;
		}

		///   !!!   end tube   !!!   ///


		///   !!!   begin fan  !!!   ///
		if ((joystick.joy2_Buttons == 1) & fanReady == true)
		{
			fanReady = false;
			if (fanRunning == false)
			{
				fanRunning = true;
				RunFan(true);
			}
			else
			{
				fanRunning = false;
				RunFan(false);
			}
		}
		else if (joystick.joy2_Buttons != 1)
		{
			fanReady = true;
		}

		if (joystick.joy2_Buttons == 2)
		{
			servo[servoTubeWinch] = 54;
		}
		else if (joystick.joy2_Buttons == 3)
		{
			servo[servoTubeWinch] = 133;
		}


		if (joystick.joy2_Buttons == 11)
		{
			wait10Msec(100);
			servo[servoTubeLift] = 255;
			wait10Msec(100);
			servo[servoTubeLift] = 135;
			wait10Msec(100);
			while(SensorValue(touch) == 1023)//drag the tube to the loader
			{
				servo[servoTubeSlide] = 0;
				wait10Msec(10);
			}
			servo[servoTubeSlide] = 127;
			wait10Msec(100);
			servo[servoTubeWinch] = 54;
		}
		///   !!!   end fan    !!!   ///


		joyX = joystick.joy1_x2;
		joyY = joystick.joy1_y2;
		joyZ = joystick.joy1_x1;
		joyDistance = sqrt( pow(joyX, 2) + pow( joyY, 2) );
		joyAngle = -57.3 * atan2(joyY, joyX);

		motor[motorFL] = -1 * joystick.joy1_y1;
		motor[motorFR] = joystick.joy1_y2;
		motor[motorBL] = -1 * joystick.joy1_y1;
		motor[motorBR] = joystick.joy1_y2;

		servo[servoSweep1] = 127;
		servo[servoSweep2] = 127;
	}
}
